import Hiwonder
import time
import Hiwonder_IIC
from HW_MechDog import MechDog

# initialize variables
uart = Hiwonder.UART(9600)
_BLE_REC_DATA = 0
_COMMAND = 0
_SEND_DATA = 0
_DATA = 0
_distance = 0
scb_distance = 0
_var_81ea_5e73_8861_6807_5fd7_4f4d = 0
_RUN_STEP = 0
run_obstacle_avoidance = 0
_var_50_69_74_63_68_89d2_5ea6 = 0
_var_52_6f_6c_6c_89d2_5ea6 = 0
mechdog = MechDog()
_var_52a8_4f5c_7ec4_7c7b_578b = 0
_ACTION_NUM = 0
_var_8fd0_52a8_65b9_5411 = 0
i2c1 = Hiwonder_IIC.IIC(1)
i2csonar = Hiwonder_IIC.I2CSonar(i2c1)
IMU = Hiwonder_IIC.MPU(i2c1)

time.sleep(1)


def start_main():
  global _BLE_REC_DATA
  global _COMMAND
  global _SEND_DATA
  global _DATA
  global _distance
  global scb_distance
  global _var_81ea_5e73_8861_6807_5fd7_4f4d
  global _RUN_STEP
  global run_obstacle_avoidance
  global _var_50_69_74_63_68_89d2_5ea6
  global _var_52_6f_6c_6c_89d2_5ea6
  global mechdog
  global _var_52a8_4f5c_7ec4_7c7b_578b
  global _ACTION_NUM
  global _var_8fd0_52a8_65b9_5411
  global i2c1
  global i2csonar

  while True:
    if uart.contains_data("CMD"):
      _BLE_REC_DATA = uart.read_uart_cmd()
      if not _BLE_REC_DATA:
        continue
      _COMMAND = uart.parse_uart_cmd(_BLE_REC_DATA)[0]
      _COMMAND = int(_COMMAND)
      if (_COMMAND==6):
        _SEND_DATA = (str("CMD|6|")+str(((str(Hiwonder.Battery_power())+str(((str("|")+str("$"))))))))
        uart.send_data(_SEND_DATA)
        continue
      if (_COMMAND==5):
        _SEND_DATA = (str("CMD|5|")+str(((str(IMU.read_angle()[0])+str(((str("|")+str(IMU.read_angle()[1])+str("|")+str("$"))))))))
        uart.send_data(_SEND_DATA)
        continue
      if (_COMMAND==4):
        _DATA = int(uart.parse_uart_cmd(_BLE_REC_DATA)[1])
        if (_DATA==1):
          _distance = round((scb_distance*10))
          if (_distance>5000):
            _distance = 5000
          _SEND_DATA = (str("CMD|4|")+str(((str(_distance)+str(((str("|")+str("$"))))))))
          uart.send_data(_SEND_DATA)
        if ((_DATA==2) and (_var_81ea_5e73_8861_6807_5fd7_4f4d==0)):
          if ((int(uart.parse_uart_cmd(_BLE_REC_DATA)[2]))==1):
            _RUN_STEP = 41
            print("bizhang")
            time.sleep(0.5)
          else:
            _RUN_STEP = 40
            print("bizhang Ting")
            time.sleep(0.5)
        continue
      if ((_COMMAND==1) and (run_obstacle_avoidance==0)):
        _DATA = int(uart.parse_uart_cmd(_BLE_REC_DATA)[1])
        if (_DATA==3):
          _DATA = int(uart.parse_uart_cmd(_BLE_REC_DATA)[2])
          if (_DATA==1):
            _var_50_69_74_63_68_89d2_5ea6 = 0
            _var_52_6f_6c_6c_89d2_5ea6 = 0
            _RUN_STEP = 131
          else:
            _RUN_STEP = 130
        if ((run_obstacle_avoidance==0) and (_var_81ea_5e73_8861_6807_5fd7_4f4d==0)):
          if (_DATA==1):
            _DATA = int(uart.parse_uart_cmd(_BLE_REC_DATA)[2])
            if (_DATA==1):
              _var_52_6f_6c_6c_89d2_5ea6+=1
              if (_var_52_6f_6c_6c_89d2_5ea6<17):
                mechdog.transform([0, 0, 0], [-1 * 1, 0, 0], 80)
            else:
              _var_52_6f_6c_6c_89d2_5ea6+=-1
              if (_var_52_6f_6c_6c_89d2_5ea6>-17):
                mechdog.transform([0, 0, 0], [1 * 1, 0, 0], 80)
          if (_DATA==2):
            _DATA = int(uart.parse_uart_cmd(_BLE_REC_DATA)[2])
            if (_DATA==1):
              _var_50_69_74_63_68_89d2_5ea6+=1
              if (_var_50_69_74_63_68_89d2_5ea6<17):
                mechdog.transform([0, 0, 0], [0, 1 * 1, 0], 80)
            else:
              _var_50_69_74_63_68_89d2_5ea6+=-1
              if (_var_50_69_74_63_68_89d2_5ea6>-17):
                mechdog.transform([0, 0, 0], [0, -1 * 1, 0], 80)
          if (_DATA==4):
            _DATA = int(uart.parse_uart_cmd(_BLE_REC_DATA)[2])
            if (_DATA==1):
              mechdog.transform([0, 0, 1 * 1], [0, 0, 0], 80)
            else:
              mechdog.transform([0, 0, -1 * 1], [0, 0, 0], 80)
          if (_DATA==5):
            mechdog.set_default_pose()
            _var_50_69_74_63_68_89d2_5ea6 = 0
            _var_52_6f_6c_6c_89d2_5ea6 = 0
          continue
      if ((_COMMAND==2) and (((run_obstacle_avoidance==0) and (_var_81ea_5e73_8861_6807_5fd7_4f4d==0)))):
        _RUN_STEP = 2
        _var_52a8_4f5c_7ec4_7c7b_578b = int(uart.parse_uart_cmd(_BLE_REC_DATA)[1])
        _ACTION_NUM = int(uart.parse_uart_cmd(_BLE_REC_DATA)[2])
        print(_ACTION_NUM)
        time.sleep(0.5)
      if ((_COMMAND==3) and (((run_obstacle_avoidance==0) and (_var_81ea_5e73_8861_6807_5fd7_4f4d==0)))):
        _RUN_STEP = 3
        _var_8fd0_52a8_65b9_5411 = int(uart.parse_uart_cmd(_BLE_REC_DATA)[1])
    else:
      time.sleep(0.03)


def start_main1():
  global uart
  global _BLE_REC_DATA
  global _COMMAND
  global _SEND_DATA
  global _DATA
  global _distance
  global scb_distance
  global _var_81ea_5e73_8861_6807_5fd7_4f4d
  global _RUN_STEP
  global run_obstacle_avoidance
  global _var_50_69_74_63_68_89d2_5ea6
  global _var_52_6f_6c_6c_89d2_5ea6
  global mechdog
  global _var_52a8_4f5c_7ec4_7c7b_578b
  global _ACTION_NUM
  global _var_8fd0_52a8_65b9_5411
  global i2csonar

  step = 0
  while True:
    if (step==0):
      step = _RUN_STEP
      _RUN_STEP = 0
      time.sleep(0.05)
    else:
      if (step==41):
        run_obstacle_avoidance = 1
        while True:
          if ((_RUN_STEP==40) or (run_obstacle_avoidance==0)):
            run_obstacle_avoidance = 0
            mechdog.move(0,0)
            i2csonar.setRGB(0,0x33,0x33,0xff)
            time.sleep(1)
            break
          if (scb_distance<10):
            i2csonar.setRGB(0,0xff,0x00,0x00)
            mechdog.move(-30,0)
            for count in range(30):
              if (_RUN_STEP==40):
                break
              time.sleep(0.1)
          else:
            if (scb_distance<40):
              i2csonar.setRGB(0,0xff,0xcc,0x00)
              mechdog.move(50,-30)
            else:
              i2csonar.setRGB(0,0xcc,0x33,0xcc)
              mechdog.move(50,0)
          time.sleep(0.02)
      if (step==131):
        _var_81ea_5e73_8861_6807_5fd7_4f4d = 1
        mechdog.homeostasis(True)
        time.sleep(2)
        while True:
          if (_RUN_STEP==130):
            _var_81ea_5e73_8861_6807_5fd7_4f4d = 0
            mechdog.homeostasis(False)
            time.sleep(2)
            break
          if not (mechdog.read_homeostasis_status()):
            uart.send_data((str("CMD|1|3|0")+str(((str("|")+str("$"))))))
            _var_81ea_5e73_8861_6807_5fd7_4f4d = 0
            break
      if (step==2):
        if (_var_52a8_4f5c_7ec4_7c7b_578b==1):
          dong_zuo_zu_yun_xing(_ACTION_NUM)
          print("action  over")
          time.sleep(0.5)
        else:
          mechdog.action_run(str(_ACTION_NUM))
          print("user action over")
          time.sleep(0.5)
      if (step==3):
        while True:
          if (_var_8fd0_52a8_65b9_5411==0):
            mechdog.move(0,0)
            break
          if (_var_8fd0_52a8_65b9_5411==1):
            mechdog.move(50,-30)
            continue
          if (_var_8fd0_52a8_65b9_5411==2):
            mechdog.move(50,-15)
            continue
          if (_var_8fd0_52a8_65b9_5411==3):
            mechdog.move(80,0)
            continue
          if (_var_8fd0_52a8_65b9_5411==4):
            mechdog.move(50,15)
            continue
          if (_var_8fd0_52a8_65b9_5411==5):
            mechdog.move(50,30)
            continue
          if (_var_8fd0_52a8_65b9_5411==6):
            mechdog.move(-40,-15)
            continue
          if (_var_8fd0_52a8_65b9_5411==7):
            mechdog.move(-40,0)
            continue
          if (_var_8fd0_52a8_65b9_5411==8):
            mechdog.move(-40,15)
            continue
      step = 0

def dong_zuo_zu_yun_xing(dong_zuo):
  if (dong_zuo==1):
    mechdog.action_run("left_foot_kick")
    time.sleep(2)
    return
  if (dong_zuo==2):
    mechdog.action_run("right_foot_kick")
    time.sleep(2)
    return
  if (dong_zuo==3):
    mechdog.action_run("stand_four_legs")
    time.sleep(2)
    return
  if (dong_zuo==4):
    mechdog.action_run("sit_dowm")
    time.sleep(2)
    return
  if (dong_zuo==5):
    mechdog.action_run("go_prone")
    time.sleep(2)
    return
  if (dong_zuo==6):
    mechdog.action_run("stand_two_legs")
    time.sleep(2)
    return
  if (dong_zuo==7):
    mechdog.action_run("handshake")
    time.sleep(2)
    return
  if (dong_zuo==8):
    mechdog.action_run("scrape_a_bow")
    time.sleep(2)
    return
  if (dong_zuo==9):
    mechdog.action_run("nodding_motion")
    time.sleep(2)
    return
  if (dong_zuo==10):
    mechdog.action_run("boxing")
    time.sleep(2)
    return
  if (dong_zuo==11):
    mechdog.action_run("stretch_oneself")
    time.sleep(2)
    return
  if (dong_zuo==12):
    mechdog.action_run("pee")
    time.sleep(2)
    return
  if (dong_zuo==13):
    mechdog.action_run("press_up")
    time.sleep(2)
    return
  if (dong_zuo==14):
    mechdog.action_run("rotation_pitch")
    time.sleep(2)
    return
  if (dong_zuo==15):
    mechdog.action_run("rotation_roll")
    time.sleep(2)
    return


def start_main2():
  global scb_distance
  global i2csonar

  time.sleep(2)
  while True:
    scb_distance = i2csonar.getDistance()
    time.sleep(0.08)

Hiwonder.startMain(start_main)
Hiwonder.startMain(start_main1)
Hiwonder.startMain(start_main2)



